#include "mission/mission.h"

namespace wr_scan
{
    Mission::Mission()
    {
        initial();

        ws_state_sub_ = nh_.subscribe("/ws_state_num", 1, &Mission::stateCallback, this);
    }

    Mission::~Mission()
    {
        point_task_->sendPointsData("stop,points");
        imu_task_->sendImuData("stop,points");
        std::cout << "发送停止信号" << std::endl;

        delete point_task_; // 释放空间
        delete imu_task_;   // 释放空间
        delete scan_task_;  // 释放空间
    }

    bool Mission::initial()
    {
        point_task_ = new PointsTask(); // 开辟空间
        imu_task_ = new ImuTask();      // 开辟空间
        scan_task_ = new ScanTask();    // 开辟空间

        ws_points_state_ = PointsState::POINTS; // 初始化为点云状态
        ws_model_state_ = ModelState::SN;       // 初始化为获取SN状态

        // 获取点云、IMU和SN码
        point_task_->sendPointsData("hello,points");
        imu_task_->sendImuData("hello,imu");
        scan_task_->sendScanData("hello,sn");
        std::cout << "发送连接信号" << std::endl;

        return true;
    }

    // 状态1：获取点云和IMU数据
    // 状态2：获取SN数据
    // 状态3：进入低功耗模式
    // 状态4：恢复正常工作模式
    void Mission::stateCallback(const std_msgs::Int32::ConstPtr &msg)
    {
        int state_num = msg->data;
        bool flag = true;
        if (state_num == 1 && flag)
        {
            std::cout << "获取雷达和IMU数据" << std::endl;

            ws_points_state_ = PointsState::POINTS;

            point_task_->sendPointsData("hello,points");
            imu_task_->sendImuData("hello,imu");

            flag = false;
        }
        else if (state_num == 2 && flag)
        {
            std::cout << "获取雷达身份码" << std::endl;

            ws_model_state_ = ModelState::SN;

            scan_task_->sendScanData("hello,sn");

            flag = false;
        }
        else if (state_num == 3 && flag)
        {
            std::cout << "进入低功耗模式" << std::endl;

            ws_model_state_ = ModelState::LOW;

            scan_task_->sendScanData("hello,low");

            flag = false;
        }
        else if (state_num == 4 && flag)
        {
            std::cout << "恢复正常工作模式" << std::endl;

            ws_model_state_ = ModelState::WORK;

            scan_task_->sendScanData("hello,work");

            flag = false;
        }
    }

    void Mission::run()
    {
        ros::Rate loop_rate(1);
        while (ros::ok())
        {
            scan_task_->sendScanData("hello,connect"); // 一直发送握手信号给雷达

            bool is_connected = scan_task_->getScanConnect(); // 获取雷达连接信号
            if (is_connected)
            {
                // 获取点云、IMU和SN码
                point_task_->sendPointsData("hello,points");
                imu_task_->sendImuData("hello,imu");
                scan_task_->sendScanData("hello,sn");

                is_connected = false;                     // 执行一次即可
                scan_task_->setScanConnect(is_connected); // 设置雷达连接信号
            }

            ros::spinOnce();
            loop_rate.sleep();
        }
    }
}
